#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Empty.h"
#include "tf/transform_datatypes.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseStamped.h"
#include "tello_baotou/Position.h"
#include <math.h>
#include <iostream>
#define PI 3.1415926
using namespace std;

double t = 0, tstart = 0;
int i, j, flag = 0;
double K = -1.3, u1[3], u2[3], u3[3], u4[3];
double tello_pos1[3], tello_pos2[3], tello_pos3[3], tello_pos4[3]; // 位置
double tello1_yaw,tello2_yaw,tello3_yaw,tello4_yaw;
double yaw1,yaw2,yaw3,yaw4;
double expectAngle1 = 0, expectAngle2 = 0, expectAngle3 = 0, expectAngle4 = 0;
double p[3];// 位置跟踪项
double h1[3] = {0}, h2[3] = {0}, h3[3] = {0}, h4[3] = {0};// 编队偏移项		
double v1[3] = {0}, v2[3] = {0}, v3[3] = {0}, v4[3] = {0};			
double r1 = 1, r2 = 1, w = 0.1745;//定义偏移半径与角速度
double n = 0.7;//最大速度
double t1 = 109, t2 = 181;//旋转时间的两个时间点
// double t1 = 40, t2 = 80;

tello_baotou::Position center_pos;//编队中心	

tello_baotou::Position expect1,expect2,expect3;

geometry_msgs::Twist cmd_vel1;
geometry_msgs::Twist cmd_vel2;
geometry_msgs::Twist cmd_vel3;
geometry_msgs::Twist cmd_vel4;

int command = 0;

void cb_readPos_1(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	double Roll,Pitch,Yaw;
	geometry_msgs::PoseStamped pos;
	pos = *msg;
	tello_pos1[0] = pos.pose.position.x;
	tello_pos1[1] = pos.pose.position.y;
	tello_pos1[2] = pos.pose.position.z;

	tf::Quaternion quat;
      	tf::quaternionMsgToTF(pos.pose.orientation,quat);
      	tf::Matrix3x3(quat).getRPY(Roll,Pitch,Yaw);
	Yaw = Yaw*180/3.14159;
	Yaw = atan2(sin(Yaw),cos(Yaw));
	tello1_yaw = Yaw; 
}

void cb_readPos_2(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	double Roll,Pitch,Yaw;
	geometry_msgs::PoseStamped pos;
	pos = *msg;

	tello_pos2[0] = pos.pose.position.x;
	tello_pos2[1] = pos.pose.position.y;
	tello_pos2[2] = pos.pose.position.z;

	tf::Quaternion quat;
    	tf::quaternionMsgToTF(pos.pose.orientation,quat);
    	tf::Matrix3x3(quat).getRPY(Roll,Pitch,Yaw);
	Yaw = Yaw*180/3.14159;
	Yaw = atan2(sin(Yaw),cos(Yaw));
	tello2_yaw = Yaw; 
}

void cb_readPos_3(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	double Roll,Pitch,Yaw;
	geometry_msgs::PoseStamped pos;
	pos = *msg;

	tello_pos3[0] = pos.pose.position.x;
	tello_pos3[1] = pos.pose.position.y;
	tello_pos3[2] = pos.pose.position.z;

	tf::Quaternion quat;
      	tf::quaternionMsgToTF(pos.pose.orientation,quat);
      	tf::Matrix3x3(quat).getRPY(Roll,Pitch,Yaw);
	Yaw = Yaw*180/3.14159;
	Yaw = atan2(sin(Yaw),cos(Yaw));
	tello3_yaw = Yaw; 
}

void cb_readPos_4(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	double Roll,Pitch,Yaw;
	geometry_msgs::PoseStamped pos;
	pos = *msg;
	tello_pos4[0] = pos.pose.position.x;
	tello_pos4[1] = pos.pose.position.y;
	tello_pos4[2] = pos.pose.position.z;

	tf::Quaternion quat;
      	tf::quaternionMsgToTF(pos.pose.orientation,quat);
      	tf::Matrix3x3(quat).getRPY(Roll,Pitch,Yaw);
	Yaw = Yaw*180/3.14159;
	Yaw = atan2(sin(Yaw),cos(Yaw));
	tello4_yaw = Yaw; 
}

void poscmd(const tello_baotou::Position::ConstPtr& msg)
{
    tello_baotou::Position p_cmd;
	p_cmd = *msg;//msg是数据整体，并不是一个指针
	p[0] = p_cmd.position_x;
	p[1] = p_cmd.position_y;
}

void cb_command(const std_msgs::Int32::ConstPtr &msg)
{
	std_msgs::Int32 cmd_data;
	cmd_data = *msg;
	command = cmd_data.data;
	cout << "receive command: " << command << endl;
	if (command == 50 && flag == 0)
	{
		tstart = ros::Time::now().toSec();
		flag = 1;
	}
}

void vel_limit(double u[],double n)
{
	int i;
	for (i = 0; i < 2; i++)
	{
		if (u[i] > n)
			u[i] = n;
		if (u[i] < -n)
			u[i] = -n;
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "tello0");
	ros::NodeHandle nh("");

	ros::Subscriber cmd_sub = nh.subscribe("/command", 10, cb_command);
	// ros::Subscriber pos_sub = nh.subscribe("/nlink_linktrack_anchorframe0",10,cb_readPos); //cb means Callback
	ros::Subscriber pos_sub1 = nh.subscribe("/tello20/pose", 10, cb_readPos_1);
	ros::Subscriber pos_sub2 = nh.subscribe("/tello21/pose", 10, cb_readPos_2);
	ros::Subscriber pos_sub3 = nh.subscribe("/tello22/pose", 10, cb_readPos_3);
	ros::Subscriber pos_sub4 = nh.subscribe("/tello24/pose", 10, cb_readPos_4);
	
	ros::Publisher center_pub = nh.advertise<tello_baotou::Position>("/tello_center", 10);
	// ros::Subscriber expectPos_sub = nh.subscribe("expectPos",10,cb_expectPos);
	// ros::Subscriber status_sub = nh.subscribe("status",10,cb_status);
	// ros::Subscriber imu_sub = nh.subscribe("imu",10,cb_imu);
	ros::Publisher vel_pub1 = nh.advertise<geometry_msgs::Twist>("/tello20/cmd_vel", 10);
	ros::Publisher vel_pub2 = nh.advertise<geometry_msgs::Twist>("/tello21/cmd_vel", 10);
	ros::Publisher vel_pub3 = nh.advertise<geometry_msgs::Twist>("/tello22/cmd_vel", 10);
	ros::Publisher vel_pub4 = nh.advertise<geometry_msgs::Twist>("/tello24/cmd_vel", 10);

	ros::Publisher takeoff_pub1 = nh.advertise<std_msgs::Empty>("/tello20/takeoff", 10);
	ros::Publisher takeoff_pub2 = nh.advertise<std_msgs::Empty>("/tello21/takeoff", 10);
	ros::Publisher takeoff_pub3 = nh.advertise<std_msgs::Empty>("/tello22/takeoff", 10);
	ros::Publisher takeoff_pub4 = nh.advertise<std_msgs::Empty>("/tello24/takeoff", 10);

	ros::Publisher land_pub1 = nh.advertise<std_msgs::Empty>("/tello20/land", 10);
	ros::Publisher land_pub2 = nh.advertise<std_msgs::Empty>("/tello21/land", 10);
	ros::Publisher land_pub3 = nh.advertise<std_msgs::Empty>("/tello22/land", 10);
	ros::Publisher land_pub4 = nh.advertise<std_msgs::Empty>("/tello24/land", 10);

	ros::Publisher tello21 = nh.advertise<tello_baotou::Position>("/tello21", 10);
	ros::Publisher tello22 = nh.advertise<tello_baotou::Position>("/tello22", 10);
	ros::Publisher tello24 = nh.advertise<tello_baotou::Position>("/tello24", 10);

	ros::Subscriber pos_cmd = nh.subscribe("pos_cmd", 10, poscmd);


	ros::Rate loop_rate(100);

	while (ros::ok())
	{
		double t = ros::Time::now().toSec() - tstart;
		// center_pos.position_x = tello_pos1[0]+tello_pos2[0]+tello_pos3[0]+tello_pos4[0];
		// center_pos.position_y = tello_pos1[1]+tello_pos2[1]+tello_pos3[1]+tello_pos4[1]; 
		// center_pos.position_x = (tello_pos2[0]+tello_pos3[0]+tello_pos4[0])/3;
		// center_pos.position_y = (tello_pos2[1]+tello_pos3[1]+tello_pos4[1])/3; 
		if(t < 3||t>1000)
		{
			center_pos.position_x = 1.5;
			center_pos.position_y = 2;
		}
		else if(t<17&&t>=3)
		{
			center_pos.position_x = 1.5;
			center_pos.position_y = 3.5;
		}
		else if(t<41&&t>=17)
		{
			center_pos.position_x = 1.5;
			center_pos.position_y = 3.5;
		}
		else if(t<81&&t>=41)
		{
			center_pos.position_x = 1.5;
			center_pos.position_y = 5.5;
		}
		else if(t<96&&t>=81)
		{
			center_pos.position_x = 1.5;
			center_pos.position_y = 8.5;
		}
		else if(t<106&&t>=96)
		{
			center_pos.position_x = 1.5;
			center_pos.position_y = 10;
		}
		else if(t<182&&t>=106)
		{
			center_pos.position_x = 4;
			center_pos.position_y = 10;
		}
		else if(t<192&&t>=182)
		{
			center_pos.position_x = 4;
			center_pos.position_y = 10;
		}
		else if(t<212&&t>=192)
		{
			center_pos.position_x = 1.5;
			center_pos.position_y = 13;
		}
		else if(t<242&&t>=212)
		{
			center_pos.position_x = 1.5;
			center_pos.position_y = 15;
		}
		else
		{
			center_pos.position_x = 1.5;
			center_pos.position_y = 18;
		}
		center_pub.publish(center_pos);

		if (command == 0) //读取初始偏航角并储存
		{	
			expectAngle1 = tello1_yaw;
			expectAngle2 = tello2_yaw;
			expectAngle3 = tello3_yaw;
			expectAngle4 = tello4_yaw;
		}
		if (command == 1) // 起飞
		{
			std_msgs::Empty takeoff_cmd;
			takeoff_pub1.publish(takeoff_cmd);
			takeoff_pub2.publish(takeoff_cmd);
			takeoff_pub3.publish(takeoff_cmd);
			takeoff_pub4.publish(takeoff_cmd);
		}
		if (command == 50)
		{
			// if (t < 10)
			// {
			// 	h1[0] = r * cos(PI / 4), h1[1] = r * sin(PI / 4);
			// 	h2[0] = r * cos(3 * PI / 4), h2[1] = r * sin(3 * PI / 4);
			// 	h3[0] = r * cos(5 * PI / 4), h3[1] = r * sin(5 * PI / 4);
			// 	h4[0] = r * cos(7 * PI / 4), h4[1] = r * sin(7 * PI / 4);
			// }
			// else if (t > 10 && t < 40)
			// {
			// 	h1[0] = r * cos(w * (t - 3) + PI / 4), h1[1] = r * sin(w * (t - 3) + PI / 4);
			// 	h2[0] = r * cos(w * (t - 3) + 3 * PI / 4), h2[1] = r * sin(w * (t - 3) + 3 * PI / 4);
			// 	h3[0] = r * cos(w * (t - 3) + 5 * PI / 4), h3[1] = r * sin(w * (t - 3) + 5 * PI / 4);
			// 	h4[0] = r * cos(w * (t - 3) + 7 * PI / 4), h4[1] = r * sin(w * (t - 3) + 7 * PI / 4);
			// }
			// else
			// {
			// 	h1[0] = cos(PI / 4), h1[1] = sin(PI / 4);
			// 	h2[0] = cos(3 * PI / 4), h2[1] = sin(3 * PI / 4);
			// 	h3[0] = cos(5 * PI / 4), h3[1] = sin(5 * PI / 4);
			// 	h4[0] = cos(7 * PI / 4), h4[1] = sin(7 * PI / 4);
			// }
			if (t <= t1 || t >= t2)
			{
				h2[0] = r1*cos(PI/3), h2[1] = r1*sin(PI/3);
				h3[0] = r1*cos(PI), h3[1] = r1*sin(PI);
				h4[0] = r1*cos(5 * PI / 3), h4[1] = r1*sin(5 * PI / 3);
				v2[0] = 0, v2[1] = 0;
				v3[0] = 0, v3[1] = 0;
				v4[0] = 0, v4[1] = 0;
				cout<<"tello偏移量1:"<<h3[0]<<" "<<h3[1]<<endl;
			}
			else if (t > t1 && t < t1)
			{
				h2[0] = r2*cos(PI/3), h2[1] = r2*sin(PI/3);
				h3[0] = r2*cos(PI), h3[1] = r2*sin(PI);
				h4[0] = r2*cos(5 * PI / 3), h4[1] = r2*sin(5 * PI / 3);
				v2[0] = 0, v2[1] = 0;
				v3[0] = 0, v3[1] = 0;
				v4[0] = 0, v4[1] = 0;
				cout<<"tello偏移量1:"<<h3[0]<<" "<<h3[1]<<endl;
			}
			else
			{
				h2[0] = r2 * cos(w * (t - t1) + PI / 3), h2[1] = r2 * sin(w * (t - t1) + PI / 3);
				h3[0] = r2 * cos(w * (t - t1) + PI), h3[1] = r2 * sin(w * (t - t1) + PI);
				h4[0] = r2 * cos(w * (t - t1) + 5 * PI / 3), h4[1] = r2 * sin(w * (t - t1) + 5 * PI / 3);
				v2[0] = -r2 * w * sin(w * (t - t1) + PI / 3), v2[1] = r2 * w * cos(w * (t - t1) + PI / 3);
				v3[0] = -r2 * w * sin(w * (t - t1) + PI), v3[1] = r2 * w * cos(w * (t - t1) + PI);
				v4[0] = -r2 * w * sin(w * (t - t1) + 5 * PI / 3), v4[1] = r2 * w * cos(w * (t - t1) + 5 * PI / 3);
				cout << "tello偏移量2:" << h3[0] << " " << h3[1] << endl;
			}

			u1[0] = K * (tello_pos1[0] - p[0] - h1[0]) + v1[0];//
			u2[0] = K * (tello_pos2[0] - p[0] - h2[0]) + v2[0];//+ B * K * (tello_pos2[0][0] - h2[0][0] - tello_pos5[0][0])  
			u3[0] = K * (tello_pos3[0] - p[0] - h3[0]) + v3[0];//+ B * K * (tello_pos3[0][0] - h3[0][0] - tello_pos5[0][0])  
			u4[0] = K * (tello_pos4[0] - p[0] - h4[0]) + v4[0];//+ B * K * (tello_pos4[0][0] - h4[0][0] - tello_pos5[0][0])  
			
			u1[1] = K * (tello_pos1[1] - p[1] - h1[0]) + v1[1];//
			u2[1] = K * (tello_pos2[1] - p[1] - h2[1]) + v2[1];//+ B * K * (tello_pos2[1][0] - h2[1][0] - tello_pos5[1][0])  
			u3[1] = K * (tello_pos3[1] - p[1] - h3[1]) + v3[1];//+ B * K * (tello_pos3[1][0] - h3[1][0] - tello_pos5[1][0])   
			u4[1] = K * (tello_pos4[1] - p[1] - h4[1]) + v4[1];//+ B * K * (tello_pos4[1][0] - h4[1][0] - tello_pos5[1][0])  

			if (t < 42 && t>18)
			{
				u4[0] = K * (tello_pos4[0] - 1.5 - h4[0]) + v4[0];
				u4[1] = K * (tello_pos4[1] - 3.5 - h4[1]) + v4[1];
			}
			vel_limit(u1,n), vel_limit(u2,n), vel_limit(u3,n), vel_limit(u4,n);

			yaw1 = K*(tello1_yaw - expectAngle1);
			yaw2 = K*(tello2_yaw - expectAngle2);
			yaw3 = K*(tello3_yaw - expectAngle3);
			yaw4 = K*(tello4_yaw - expectAngle4);


			expect1.position_x = p[0] + h2[0];
			expect2.position_x = p[0] + h3[0];
			expect3.position_x = p[0] + h4[0];

			expect1.position_y = p[1] + h2[1];
			expect2.position_y = p[1] + h3[1];
			expect3.position_y = p[1] + h4[1];
			if (t < 42 && t>18)
			{
				expect3.position_x = 1.5 + h4[0];
				expect3.position_y = 3.5 + h4[1];
			}
			tello21.publish(expect1);
			tello22.publish(expect2);
			tello24.publish(expect3);


			// 确定无人机的速度输入
			cmd_vel1.linear.x = u1[0];
			cmd_vel1.linear.y = u1[1];
			cmd_vel2.linear.x = u2[0];
			cmd_vel2.linear.y = u2[1];
			cmd_vel3.linear.x = u3[0];
			cmd_vel3.linear.y = u3[1];
			cmd_vel4.linear.x = u4[0];
			cmd_vel4.linear.y = u4[1];
			// cmd_vel1.angular.z = yaw1;
			// cmd_vel2.angular.z = yaw2;
			// cmd_vel3.angular.z = yaw3;
			// cmd_vel4.angular.z = yaw4;

			// cout << "x1_pos: " << tello_pos1[0][0] << " y1_pos: " << tello_pos1[1][0] << endl;
			// cout << "x1_vel: " << cmd_vel1.linear.x << " y1_vel: " << cmd_vel1.linear.y << endl;
			// cout << "x2_pos: " << tello_pos2[0][0] << " y2_pos: " << tello_pos2[1][0] << endl;
			// cout << "x2_vel: " << cmd_vel2.linear.x << " y2_vel: " << cmd_vel2.linear.y << endl;
			// cout << "x3_pos: " << tello_pos3[0][0] << " y3_pos: " << tello_pos3[1][0] << endl;
			// cout << "x3_vel: " << cmd_vel3.linear.x << " y3_vel: " << cmd_vel3.linear.y << endl;
			// cout << "x4_pos: " << tello_pos4[0][0] << " y4_pos: " << tello_pos4[1][0] << endl;
			// cout << "x4_vel: " << cmd_vel4.linear.x << " y4_vel: " << cmd_vel4.linear.y << endl;

			vel_pub1.publish(cmd_vel1);
			vel_pub2.publish(cmd_vel2);
			vel_pub3.publish(cmd_vel3);
			vel_pub4.publish(cmd_vel4);
			if (command == 9) // 降落
			{
				std_msgs::Empty land_cmd;
				land_pub1.publish(land_cmd);
				land_pub2.publish(land_cmd);
				land_pub3.publish(land_cmd);
				land_pub4.publish(land_cmd);
			}
		}
		if (command == 3) // 指令速度给0
		{
			cmd_vel1.linear.x = 0;
			cmd_vel1.linear.y = 0;
			cmd_vel2.linear.x = 0;
			cmd_vel2.linear.y = 0;
			cmd_vel3.linear.x = 0;
			cmd_vel3.linear.y = 0;
			cmd_vel4.linear.x = 0;
			cmd_vel4.linear.y = 0;

			vel_pub1.publish(cmd_vel1);
			vel_pub2.publish(cmd_vel2);
			vel_pub3.publish(cmd_vel3);
			vel_pub4.publish(cmd_vel4);
		}
		if (command == 9) // 降落
		{
			cmd_vel1.linear.x = 0;
			cmd_vel1.linear.y = 0;
			cmd_vel2.linear.x = 0;
			cmd_vel2.linear.y = 0;
			cmd_vel3.linear.x = 0;
			cmd_vel3.linear.y = 0;
			cmd_vel4.linear.x = 0;
			cmd_vel4.linear.y = 0;

			vel_pub1.publish(cmd_vel1);
			vel_pub2.publish(cmd_vel2);
			vel_pub3.publish(cmd_vel3);
			vel_pub4.publish(cmd_vel4);
			std_msgs::Empty land_cmd;
			land_pub1.publish(land_cmd);
			land_pub2.publish(land_cmd);
			land_pub3.publish(land_cmd);
			land_pub4.publish(land_cmd);
		}
		ros::spinOnce();
		loop_rate.sleep();
	}
	return 0;
}